#include "StimDot.h"

#include <iostream>
#include <Helper.h>
#include <Random.h>
#if STIM_DOT_SAVE_DOTS
    #include <FileIO.h>
#endif


CStimDot::CStimDot(const enum Stims::StimType& eStimType_, const enum Stims::ObjType& eObjType_)
    :
CStimBase(eStimType_),
CStimObj(eObjType_),
_nDotLifeTime(0),   // default unlimited lifetime
_nFormOffset(0)     // default no Glass pattern stimulus
{
}
CStimDot::~CStimDot(void)
{
    glDeleteLists(_uPointList,1);
}


void CStimDot::Reset()
{
    if (!glIsList(_uPointList))
        _uPointList = glGenLists(1);
    
    // compute frustum definition
    double dFOVy_RAD_2  = _dFOVy/2.*DEG2RAD;  // half of FOVy in radians
    double dFOVx_RAD_2  = atan(tan(dFOVy_RAD_2)*_dAspectRatio);
    _dTanFOVy_2         = tan(dFOVy_RAD_2);
    _dTanFOVx_2         = tan(dFOVx_RAD_2);

    //// Object type specific inits
    // create a single in stance of the object in a display list
    InitObj();
    // correct near plane in the case we're drawing a 3D object.
    // We have to make sure these objects are not cut by the near plane
    switch (_eObjType)
    {
    case Stims::GLpoint:
        // nothing
        break;

    case Stims::Sphere:
        _dNearPlane += _dSphereRadius;
        break;

    case Stims::Disc:
        _dNearPlane += _dDiscRadius;
        break;

    case Stims::Post:
        _dNearPlane += _dPostRadius;
        break;

    case Stims::PostSphere:
        _dNearPlane += _dPostRadius > _dSphereRadius ? _dPostRadius : _dSphereRadius;
        break;

    default:
        throw std::exception("StimDot::Reset(): Init for this Object Type not implemented");
        break;
    }

    // Check settings regarding number of points ans number of areas to put them in
    if (_nNPoint < _nNArea)
        throw exception("Dot stimulus algorithms: Number of areas larger than number of visible points");

    // Allocate memory
    InitVectors();

    // initialize number of dots per area
    ComputeDistribution();

    _bFirstFrame = true;

#if STIM_DOT_SAVE_DOTS
    _nCurrentFrame = 0;
    _nNextPointID = 1; // start at one to make the MATLAB code easier
#endif                  
}

void CStimDot::InitVectors()
{
    // always
    _vvPointList    .clear();
    _vvPointList    .resize(_nNArea);
    _vNumDot        .clear();
    _vNumDot        .resize(_nNArea);
    _vHalfLengthX   .clear();
    _vHalfLengthX   .resize(_nNArea);
    _vHalfLengthY   .clear();
    _vHalfLengthY   .resize(_nNArea);
    _vHalfLengthZ   .clear();
    _vHalfLengthZ   .resize(_nNArea);

    // limited lifetime
    _vvPointLifeList.clear();
    _vvPointLifeList.resize(_nNArea);

    // when saving dots to file, vector of point IDs
#if STIM_DOT_SAVE_DOTS
    _vvPointIDs.clear();
    _vvPointIDs.resize(_vvPointList.size());
#endif
}

void CStimDot::ComputeDistribution()
{
    // initialize
    std::vector<double> vDensity(_nNArea,0.);

    // we need to slice the frustum into depth ranges. A different number of dots
    // will then be generated in each range to keep the distribution consistent
    double dStep        = (_dFarPlane-_dNearPlane)/_nNArea;

    double dNumPointPerDepthSlice = _nNPoint/double(_nNArea);

    for (int i=0;i<_nNArea;i++)
    {
        // ground
        double dAreaSmall   = ComputeSlice(_dNearPlane +   i  *dStep,-1);
        double dAreaLarge   = ComputeSlice(_dNearPlane + (i+1)*dStep,i);
        double dArea        = (dAreaLarge - dAreaSmall);

        vDensity[i]         = dNumPointPerDepthSlice/dArea;
    }

    // as areas are overlapping, subtract this density from the density of the next larger area
    for (unsigned int i=0; i<vDensity.size()-1; i++)
        vDensity[i] -= vDensity[i+1]; 

    // given density and area, we can compute how many dots each square should have
    double dAreaSmall = ComputeSlice(_dNearPlane,-1);
    for (int i=0; i<_nNArea; i++)
    {
        double dAreaLarge  = ComputeSlice(_dNearPlane + (i+1)*dStep,-1);
        _vNumDot[i] = int((dAreaLarge-dAreaSmall) * vDensity[i] + .5);
    }

    // number of dots in stimulus
    int nCount = 0;
    for (int i=0; i<_nNArea; i++)
        nCount += _vNumDot[i];

    std::cout << nCount << " dots in stimulus" << std::endl;
}

void CStimDot::Update(const CMatrix<double>& mEyePos_, const double& dYaw_)
{
    // update
    PerFramePrecompute(mEyePos_,dYaw_);
    UpdateAllPointLists();
    DrawToDisplayList();

    if (_bFirstFrame)
        _bFirstFrame = false;   // not first frame anymore

#if STIM_DOT_SAVE_DOTS
    SaveToFile();

    _nCurrentFrame++;
#endif
}

void CStimDot::PerFramePrecompute(const CMatrix<double>& mEyePos_, const double& dYaw_)
{
    //// precompute observer state
    
    // rotation matrices, we need them to generate points in the world and check whether they are inside the frustum
    // previous frame
    if (!_bFirstFrame)
        _mRotationOT = _mRotationNT;    // save old rotation matrix

    // current frame
    double dAxis[3] = {0,1,0}; // rotate about Y axis, observer gaze angle (yaw) relative to straight along Z
    // for generating points
    _mRotationNG.SetRot( dYaw_,dAxis);
    // for testing if points inside
    _mRotationNT.SetRot(-dYaw_,dAxis);

    // store current and previous eye point
    // previous frame
    if (!_bFirstFrame)
        _mEyePosO = _mEyePosN;    // save old eye position

    // current frame
    _mEyePosN   = mEyePos_;
}

void CStimDot::UpdateAllPointLists()
{
    // for every rectangle, update points inside each cuboid
    for (unsigned int i=0;i<_vvPointList.size();i++)
        UpdatePointList(_vHalfLengthX[i],_vHalfLengthY[i],_vHalfLengthZ[i],_vNumDot[i], i);
}

void CStimDot::DrawToDisplayList()
{
    glNewList(_uPointList,GL_COMPILE);

    for (int i=0; i<(int)_vvPointList.size(); i++)
        for (int nNum=0; nNum<(int)_vvPointList[i].size()/3; nNum++)
        {
            glMatrixMode(GL_MODELVIEW);
            glPushMatrix();
            glTranslated(_vvPointList[i][nNum*3],_vvPointList[i][nNum*3+1],_vvPointList[i][nNum*3+2]);

            glCallList(_uObj);

            glPopMatrix();
        }
    glEndList();
}

void CStimDot::Draw()
{
    glCallList(_uPointList);
}


// helpers
void CStimDot::UpdatePointList(const double& dRadiusX_,const double& dRadiusY_,const double& dRadiusZ_,const int& nNumWeWant_, unsigned int& uIdx_)
{
    // copy old
    vector<double> vOldPointList    = _vvPointList[uIdx_];
    vector<int> vOldPointLifeList   = _vvPointLifeList[uIdx_];      // dammit, we cant put this inside an if and because of that we always have to copy empty vectors..

    // clear the output lists
    _vvPointList[uIdx_].clear();
    _vvPointList[uIdx_].reserve(nNumWeWant_*3);
    int nSizeOldLeft = 0;

    if (_nDotLifeTime>0)
    {
        _vvPointLifeList[uIdx_].clear();
        _vvPointLifeList[uIdx_].reserve(nNumWeWant_);
    }
    int nExpiredDots = 0;

#if STIM_DOT_SAVE_DOTS
    vector<int> vOldPointIDs = _vvPointIDs[uIdx_];
    _vvPointIDs[uIdx_].clear();
    _vvPointIDs[uIdx_].reserve(nNumWeWant_);
#endif

    if (_bFirstFrame) // if this is the first frame, we're done
    {
        // generate according to precomputed density, observer position and viewing direction
        GenerateRandomFrustum(dRadiusX_,dRadiusY_,dRadiusZ_,nNumWeWant_, uIdx_, true); // last doesn't matter, see if for checking if valid points
        return;
    }


    // now we'll generate the output list:
    // points in the old list that are still inside the area are kept,
    // added to that are points in the new list that are generated in
    // locations not in the old area
    CMatrix<double> mPoint;
    mPoint.SetSize(3,1,0);


    for (int i=0; i<(int)vOldPointList.size()/3; i++)
    {
        mPoint(0) = vOldPointList[i*3+0];
        mPoint(1) = vOldPointList[i*3+1];
        mPoint(2) = vOldPointList[i*3+2];

        if (InsideFrustum(mPoint,dRadiusX_,dRadiusY_,dRadiusZ_,_mRotationNT,_mEyePosN))     // check old still inside new
            if (_nDotLifeTime==0 || (_nDotLifeTime>0 && vOldPointLifeList[i]>0))            // If limited lifetime, check dot not expired)
            {
                // good point, copy into output
                nSizeOldLeft++;
                _vvPointList[uIdx_].push_back(mPoint(0));
                _vvPointList[uIdx_].push_back(mPoint(1));
                _vvPointList[uIdx_].push_back(mPoint(2));

                if (_nDotLifeTime>0)
                    _vvPointLifeList[uIdx_].push_back(vOldPointLifeList[i]-1);

#if STIM_DOT_SAVE_DOTS
                _vvPointIDs[uIdx_].push_back(vOldPointIDs[i]);
#endif
            }
            else
                nExpiredDots++;
    }

    // regenerate expired dots inside the old frustum
    if (_nDotLifeTime>0)
        GenerateRandomFrustum(dRadiusX_,dRadiusY_,dRadiusZ_,nExpiredDots, uIdx_, true);

    // generate dots that left the frustum such that not inside the old frustum
    GenerateRandomFrustum(dRadiusX_,dRadiusY_,dRadiusZ_,nNumWeWant_ - nSizeOldLeft - nExpiredDots, uIdx_, false);
}

void CStimDot::GenerateRandomFrustum(const double& dRadiusX_,const double& dRadiusY_,const double& dRadiusZ_, const int& nNumWeWant_, unsigned int& uIdx_, const bool& bInsideOld_)
{
    /** We generate points inside the frustum at origin
        and then translate and rotate them.
    */

    CMatrix<double> mPoint;
    mPoint.SetSize(3,1,0);

    int nNNewValidPoints = 0;
    while (nNNewValidPoints<nNumWeWant_)
    {
        SamplePoint(dRadiusZ_, mPoint);
        
        // rotate and translate to world position
        mPoint = _mRotationNG*mPoint + _mEyePosN;

        // test if not inside old frustum
        if (
            _bFirstFrame ||                                                                                     // if first frame, we don't need to test if in old frustum
            (!bInsideOld_ && !InsideFrustum(mPoint,dRadiusX_,dRadiusY_,dRadiusZ_,_mRotationOT, _mEyePosO)) ||   // not inside old frustum
            ( bInsideOld_ &&  InsideFrustum(mPoint,dRadiusX_,dRadiusY_,dRadiusZ_,_mRotationOT, _mEyePosO))      //     inside old frustum
           )
        {
            // a valid point! Store in output
            nNNewValidPoints++;

            _vvPointList[uIdx_].push_back(mPoint(0));
            _vvPointList[uIdx_].push_back(mPoint(1));
            _vvPointList[uIdx_].push_back(mPoint(2));

            if (_nDotLifeTime>0)
                if (_bFirstFrame)
                    _vvPointLifeList[uIdx_].push_back(RandLim(1,_nDotLifeTime)); // random lifetime
                else
                    _vvPointLifeList[uIdx_].push_back(_nDotLifeTime);

#if STIM_DOT_SAVE_DOTS
            _vvPointIDs[uIdx_].push_back(_nNextPointID++);
#endif
        }
    }
}

bool CStimDot::InsideFrustum(const CMatrix<double>& mPoint_, const double& dRadiusX_,const double& dRadiusY_,const double& dRadiusZ_,const CMatrixR& mRotMat_,const CMatrix<double>& mEyePos_)
{
    // rotate and translate back to origin
    CMatrix<double> mOriginPoint = mRotMat_*(mPoint_ - mEyePos_);

    if(abs(mOriginPoint(0))<dRadiusX_ && abs(mOriginPoint(1))<dRadiusY_ && mOriginPoint(2)>-dRadiusZ_ && mOriginPoint(2)<-_dNearPlane)// test if inside frustum bounding box
    {
        if(
            abs(mOriginPoint(0)/mOriginPoint(2)) < _dTanFOVx_2 &&   // test if inside frustum
            abs(mOriginPoint(1)/mOriginPoint(2)) < _dTanFOVy_2
            )
            return InsideFrustumDerived(mOriginPoint); //true;
        else 
            return false;
    }
    else
        return false;
}

#if STIM_DOT_SAVE_DOTS
void CStimDot::SaveToFile()
{
    CFileIO cFIO;
    CMatrix<double> mPoint, mPointPlusID;
    mPoint.SetSize(3,1,0);
    mPointPlusID.SetSize(1,4,0);

    char cFileName[200];
    sprintf_s(cFileName,200,"dot_positions%04d.txt",_nCurrentFrame);

    // create file
    if(cFIO.OpenFileWrite(cFileName, "data")) // hard-code directory "data" for output
    {
        for (unsigned int i=0; i<_vvPointList.size(); i++)
            for (unsigned int j=0; j<_vvPointList[i].size()/3; j++)
            {
                // get point
                mPoint(0) = _vvPointList[i][j*3+0];
                mPoint(1) = _vvPointList[i][j*3+1];
                mPoint(2) = _vvPointList[i][j*3+2];

                // rotate to origin
                mPoint = _mRotationNT*(mPoint - _mEyePosN);

                // add ID and prepare for write (row vector is nicer than column vector)
                mPointPlusID(0) = _vvPointIDs[i][j];
                mPointPlusID.Set(0,1,mPoint.Transpose());

                // write
                cFIO.WriteToFile(mPointPlusID);
            }
    }
}
#endif